#ifndef RSIM_SYSTEMDATAUTILS_H
#define RSIM_SYSTEMDATAUTILS_H

/* -------------------------------------------------------------------------- *
 * File: SystemDataUtils.h                                                    *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

namespace RSIM{

class Constraint;
class MultibodySystem;
template <class T> class List;

class SystemDataUtils{
        public:
                SystemDataUtils(){}

        protected:
                bool areConstraintsCoupled(const Constraint* ptr1,
                                        const Constraint* ptr2);

                // This calculates the common parent for each constraints
                // Also sets the path from the constained body to the common parent
                int calcCommonParentForConstraint(const List<MultibodySystem*>& system,
                                                Constraint *cPtr);                                                                  
                                                
                void sortCoupledConstraintList(const int& nConstraints,
                                        const List<List<int> >& cl,
                                        List<List<int> >& ccouple)const;
                                        
                void sortCoupledConstraintList_1(const int& nConstraints,
                                                const List<List<int> >& cl,
                                                List<List<int> >& ccouple)const;
                                                
                bool q_check(	const List<List<int> >& ccouple,
                                const int& i,
                                const int& j,
                                int *i_index);
                                
                void recursiveSortCCList(const List<List<int> >& list,
                                        const int& element,
                                        const List<List<int> >& ccouple,
                                        List <int>& coupled_indices)const;
        private:
                

                void findElement(const List<List<int> >& list, 
                                const int& element,
                                int& index1,
                                int& index2)const;
                                
                bool areConstraintBranchesOverlapping(	const MultibodySystem *MBSPtr,
                                                        const int& parent_nd_1,
                                                        const int& common_parent_1,
                                                        const int& parent_nd_2,
                                                        const int& common_parent_2
                                                        );
}; // class SystemDataUtils
}  // namespace RSIM

#endif